# -*- coding:utf-8 -*-

"""  着陆时候的高度与侧距的波形图 """
import configparser
import logging
import sys

import pyqtgraph as pg
from PyQt5 import QtWidgets

from data.DataHandler import datahandler
from data.defines import *
from data.qtutils import *

file_path_mac = os.getcwd() + '/'
file_path_win = os.getcwd() + '\\'


class LandPlotPad(QWidget):
    land_state = 0
    land_state_counter = 0
    calculating_alt = 0.0  # 解算高度
    alt_set = 0.0  # 给定高度
    fly_dis = 0.0  # 待飞距
    offset_dis = 0.0  # 侧偏距
    pitch_angle = 0.0  # 俯仰角
    fly_dis_offset_dis_dict_list = []
    frame_data = [None, None, None]

    def __init__(self):
        super().__init__()
        self.setWindowFlags(self.windowFlags() | Qt.WindowStaysOnTopHint)
        self.initUI()
        self.initData()

        datahandler.sig_RecvFccSystmData.connect(self.recvFccData)
        # print("land pad inited")

    def initUI(self):
        self.setWindowTitle('着陆波形')
        # self.setWindowFlags(self.windowFlags() | Qt.Tool)
        self.resize(640, 520)
        mainlayout = QVBoxLayout(self)
        infolayout = QHBoxLayout()
        mainlayout.addLayout(infolayout)
        chartlayout = QHBoxLayout()
        mainlayout.addLayout(chartlayout)

        self.label_alt_set = QLabel('给定高度:')
        self.label_alt = QLabel('高度:')
        self.label_pitch_angle = QLabel('俯仰角:')
        self.label_off_dis = QLabel('侧偏距:')
        self.label_fly_dis = QLabel('待飞距:')
        infolayout.addWidget(self.label_alt_set)
        infolayout.addSpacing(10)
        infolayout.addWidget(self.label_alt)
        infolayout.addSpacing(10)
        infolayout.addWidget(self.label_pitch_angle)
        infolayout.addSpacing(10)
        infolayout.addWidget(self.label_fly_dis)
        infolayout.addSpacing(10)
        infolayout.addWidget(self.label_off_dis)

        self.height_pw = pg.PlotWidget(self)
        self.height_pw.setLabel('left', '高度', units='m')
        self.height_pw.setLabel('bottom', '待飞距', units='m')
        chartlayout.addWidget(self.height_pw)
        self.sideoffset_pw = pg.PlotWidget(self)
        self.sideoffset_pw.setLabel('left', '待飞距', units='m')
        self.sideoffset_pw.setLabel('bottom', '侧偏距', units='m')
        chartlayout.addWidget(self.sideoffset_pw)

        self.height_pw.addLegend(size=(30, 25), offset=(150, 5))
        self.sideoffset_pw.addLegend(size=(30, 25), offset=(150, 5))
        # 预设高度
        self.pre_height_plot = self.height_pw.plot(name='给定高度')
        self.pre_height_plot.setPen((66, 244, 140))
        # 实际高度
        self.real_height_plot = self.height_pw.plot(name='高度')
        self.real_height_plot.setPen((92, 65, 244))

        # 测偏距离
        self.sideoffset_plot = self.sideoffset_pw.plot(name='侧偏距')
        self.sideoffset_plot.setPen((92, 65, 244))

        # 跑道设置
        # 读取跑道设置
        current = os.getcwd()
        if current.startswith('/'):
            filename = file_path_mac + runway_set_filename
        else:
            filename = file_path_win + runway_set_filename

        width = None
        if os.path.exists(filename):
            cf = configparser.ConfigParser()
            try:
                cf.read(filename)
                width = cf.get(FILE_LEVEL_RUNWAY, FILE_ITEM_RUNWAY_WIDTH, fallback=None)
            except BaseException as e:
                pass
        if width is None:
            width = 120
        width = int(float(width) / 2)
        self.left_runway_line = pg.InfiniteLine(pos=-width, angle=90)
        self.right_runway_line = pg.InfiniteLine(pos=width, angle=90)
        self.central_runway_line = pg.InfiniteLine(pos=0, angle=90, pen=(255, 255, 255))
        self.sideoffset_pw.addItem(self.left_runway_line)
        self.sideoffset_pw.addItem(self.right_runway_line)
        self.sideoffset_pw.addItem(self.central_runway_line)

    def initData(self):
        self.fly_dis_list = []
        self.offset_dis_list = []
        self.alt_set_list = []
        self.calculating_alt_list = []

    def recvFccData(self, data):
        # 根据接收到的舵系统数据更新UI
        try:
            route_flight = data["route_flight"]
            dx_m = route_flight["dx_m"]
            dy_m = route_flight["dy_m"]
            dh_m = route_flight["dh_m"]
            dpsi_deg = route_flight["dpsi_deg"]

            control_mode = data["control_mode"]
            in_land_mode = control_mode["in_land_mode"]  # 着陆状态： 1：着陆（绿灯） 0：（灰色）

            control_cmd = data["control_cmd"]
            phi_cmd_deg = control_cmd["phi_cmd_deg"]
            theta_cmd_deg = control_cmd["theta_cmd_deg"]
            height_cmd_m = control_cmd["height_cmd_m"]

            sensor_fusion = data["sensor_fusion"]
            theta_deg = sensor_fusion["theta_deg"]
            phi_deg = sensor_fusion["phi_deg"]
            altitude_m = sensor_fusion["altitude_m"]
            ref_height_m = sensor_fusion["ref_height_m"]

            self.land_state = in_land_mode

            if int(self.land_state) == 1:
                self.land_state_counter += 1
            else:
                self.land_state_counter = 0

            if int(self.land_state) == 1 and self.land_state_counter > 10:
                self.calculating_alt = altitude_m
                self.label_alt.setText('高度: %.1fm' % self.calculating_alt)
                self.frame_data[1] = self.calculating_alt

                self.pitch_angle = theta_deg
                self.label_pitch_angle.setText('俯仰角: %.1fdeg' % self.pitch_angle)

                self.fly_dis = -dx_m
                if self.frame_data[0] is not None:
                    if self.frame_data[1] is not None and self.frame_data[2] is not None:
                        self.paint_height_plot()
                else:
                    self.frame_data[0] = self.fly_dis
                self.paint_offset_dist_plot(self.fly_dis, dy_m)
                self.label_fly_dis.setText('待飞距: %dm' % dx_m)
                self.label_off_dis.setText('侧偏距: %.1fm' % dy_m)

                self.alt_set = height_cmd_m
                self.label_alt_set.setText('给定高度: %dm' % self.alt_set)
                self.frame_data[2] = self.alt_set

            else:
                self.resetCacheFrameData()
                self.clearPlotData()
                self.clearPlot()

        except Exception as e:
            logging.error(repr(e))

    def paint_height_plot(self):
        self.fly_dis_list.append(self.frame_data[0])
        self.calculating_alt_list.append(self.frame_data[1])
        self.alt_set_list.append(self.frame_data[2])
        self.resetCacheFrameData()
        if not (len(self.fly_dis_list) == len(self.calculating_alt_list) == len(self.alt_set_list)):
            self.clearPlotData()
            return

        self.pre_height_plot.setData(x=self.fly_dis_list, y=self.alt_set_list)
        self.real_height_plot.setData(x=self.fly_dis_list, y=self.calculating_alt_list)

    def clearPlotData(self):
        self.fly_dis_list.clear()
        self.calculating_alt_list.clear()
        self.alt_set_list.clear()
        self.fly_dis_offset_dis_dict_list.clear()

    def clearPlot(self):
        self.pre_height_plot.clear()
        self.real_height_plot.clear()
        self.sideoffset_plot.clear()

    def closeEvent(self, e):
        super(LandPlotPad, self).closeEvent(e)
        self.clearPlot()

    def resetCacheFrameData(self):
        for i in range(3):
            self.frame_data[i] = None

    def paint_offset_dist_plot(self, fly_dist_v, offset_dist_v):
        self.fly_dis_offset_dis_dict_list.append({'x': offset_dist_v, 'y': fly_dist_v})
        self.sideoffset_plot.setData(self.fly_dis_offset_dis_dict_list)


# 地面站软件的入口程序
if __name__ == '__main__':
    # 启动地面站主界面
    app = QtWidgets.QApplication(sys.argv)
    gcs_main_window = QtWidgets.QMainWindow()
    ui = LandPlotPad()
    # ui.setupUi(gcs_main_window)
    # ui.gui_init()
    # gcs_main_window.show()  # 显示窗口
    ui.show()
    # ui.close()
    sys.exit(app.exec_())
